#include <stdio.h>
#include <inttypes.h>
#include <math.h>
#include <float.h>
#include "color_detect.h"
#include "color_converter.h"
#include "solve.h"

//                           2-----------2------------1
//                           | U1(0)   U2(1)   U3(2)  |
//                           |                        |
//                           3 U4(3)   U5(4)   U6(5)  1
//                           |                        |
//                           | U7(6)   U8(7)   U9(8)  |
//  2-----------3------------3-----------0------------0-----------1------------1------------2------------2
//  | L1(36)  L2(37)  L3(38) | F1(18)  F2(19)  F3(20) | R1(9)   R2(10)  R3(11) |  B1(45)  B2(46)  B3(47) |
//  |                        |                        |                        |                         |
// 11 L4(39)  L5(40)  L6(41) 9 F4(21)  F5(22)  F6(23) 8 R4(12)  R5(13)  R6(14) 10 B4(48)  B5(49)  B6(50) 11
//  |                        |                        |                        |                         |
//  | L7(42)  L8(43)  L9(44) | F7(24)  F8(25)  F9(26) | R7(15)  R8(16)  R9(17) |  B7(51)  B8(52)  B9(53) |
//  3-----------7------------5-----------4------------4-----------5------------7------------6------------3
//                           | D1(27)  D2(28)  D3(29) |
//                           |                        |
//                           7 D4(30)  D5(31)  D6(32) 5
//                           |                        |
//                           | D7(33)  D8(34)  D9(35) |
//                           6-----------6------------7

#ifndef M_PIf
#define M_PIf		3.14159265358979323846f
#endif
int color_detect(uint8_t rgb_color_in[54][3], FILE *fp, char solution[80])
{
    // 调整色块顺序
    uint8_t rgb_color[54][3];
    const int map[54] = {
        47,50,53,46,49,52,45,48,51,
        0, 1, 2, 3, 4, 5, 6, 7, 8,
        35,34,33,32,31,30,29,28,27,
        26,25,24,23,22,21,20,19,18,
        44,43,42,41,40,39,38,37,36,
        9, 10,11,12,13,14,15,16,17,
    };
    for(int i=0; i<54; i++)
    {
        int src_index = map[i];
        rgb_color[i][0] = rgb_color_in[src_index][0];
        rgb_color[i][1] = rgb_color_in[src_index][1];
        rgb_color[i][2] = rgb_color_in[src_index][2];
        printf("%d, %d, %d\n", rgb_color[i][0], rgb_color[i][1], rgb_color[i][2]);
    }
    report_start(fp);
    report_cube(fp, rgb_color, "色块原始颜色:", 0);
    // 转换为HSV色彩空间
    float hsv_color[54][3];
    for(int i=0; i<54; i++)
    {
        rgb2hsv(rgb_color[i][0], rgb_color[i][1], rgb_color[i][2],
                &hsv_color[i][0], &hsv_color[i][1], &hsv_color[i][2]);
        //printf("%d %d %d %d => %.2f %.2f %.2f\n", i, rgb_color[i][0], rgb_color[i][1], rgb_color[i][2],
        //        hsv_color[i][0], hsv_color[i][1], hsv_color[i][2]);
    }
    int index[54];
    // 寻找白色,挑选饱和度S最低的9个色块,将索引号存放在index[45] - index[53]
    for(int i=0; i<54; i++)
    {
        index[i] = i;
    }
    // 冒泡排序,只排最低的9个
    for(int i=0; i<9; i++)
    {
        for(int j=0; j<54-i-1;j++)
        {
            if(hsv_color[index[j]][1] < hsv_color[index[j+1]][1])
            {
                int tmp = index[j];
                index[j] = index[j+1];
                index[j+1] = tmp;                
            }
        }
    }
    // 检测有没有白色中心块
    int white_center_index = -1;
    for(int i=45; i<54; i++)
    {
        //printf("index = %d, s=%.2f\n",index[i], hsv_color[index[i]][1]);
        if(index[i] % 9 == 4)
        {
            white_center_index = index[i];
            printf("Find white center, index=%d\n", white_center_index);
        }
    }
    if(white_center_index == -1)
    {
        printf("Error: Can not find find white center\n");
        return -1;
    }
    // 1 选择魔方的中心块颜色作为初始的聚类中心
    float h_center[5]     = {0};
    float h_center_old[5] = {-1,0,0,0,0};
    int group[5][45]      = {0};
    int group_count[5]    = {0};
    int h_center_index    = 0;
    for(int i=0; i<6; i++)
    {
        int color_index = 9 * i + 4;
        // 白色已经挑选好了,不需要再处理了
        if(color_index != white_center_index)
        {
            h_center[h_center_index++] = hsv_color[color_index][0];
        }
    }
    // 2 计算其他颜色到聚类中心的距离,划分到最接近的
    const int MAX_LOOP = 10;
    for(int loop=0; loop<MAX_LOOP; loop++)
    {
        for (int j = 0; j < 5; j++)
        {
            //printf("loop %d group %d:center=%f\n", loop, j, h_center[j]);
            group_count[j] = 0;
        }
        for(int i=0; i<45; i++)
        {
            // index[i] 0 - 44 没有白色色块, 45 - 53全是白色色块
            float h = hsv_color[index[i]][0];
            float min_dist = FLT_MAX;
            int min_dist_group = 0;
            for(int j=0; j<5; j++)
            {
                float h0 = h_center[j];
                // 要计算这两个角度之间的绝对值差，首先需要处理两个角度间的差值使其落在0到360度之间，
                // 因为简单地做差可能得出的结果会超过这个范围，例如当h0 = 350而h = 10时，它们的差应该是20度而不是340度。
                float diff = fmodf(fabsf(h - h0), 360.0f); // 先求差值并取绝对值，然后取模360度
                float dist = diff > 180.0f ? 360.0f - diff : diff; // 如果差值大于180度，则补足360度
                if(dist < min_dist)
                {
                    min_dist = dist;
                    min_dist_group = j;
                }
            }
            group[min_dist_group][group_count[min_dist_group]] = index[i];
            group_count[min_dist_group] += 1;
        }
        // 3 重新计算每个聚类中所有点的平均值,作为新的中心
        for(int j=0; j<5; j++)
        {
            float sum_cos = 0, sum_sin = 0;
            for(int i=0; i<group_count[j]; i++)
            {
                int hsv_color_index = group[j][i];
                //printf("%.2f, ",hsv_color[hsv_color_index][0]);
                // 需要注意的是359和1的平均值是0，而不是180。直接累加求平均的算法是不行的。
                // avg(a) = atan( avg(sin(a)) / avg(cos(a)) )
                float a = hsv_color[hsv_color_index][0] * (M_PIf / 180.0f);
                sum_cos += cosf(a);
                sum_sin += sinf(a);
            }
            //printf(" count of group %d : %d\n",j,group_count[j]);
            if(group_count[j] > 0)
            {
                float a = atan2f(sum_sin, sum_cos) * (180.0f / M_PIf);
                h_center[j] = fmodf(a + 360.0f, 360.0f);
            }
        }
        // 4 检查聚类中心是否有变化
        int change = 0;
        for (int j = 0; j < 5; j++)
        {
            if (h_center_old[j] != h_center[j])
            {
                change++;
            }
        }
        if (change == 0)
        {
            printf("break on loop %d.\n", loop);
            break;
        }
        else
        {
            for (int j = 0; j < 5; j++)
            {
                h_center_old[j] = h_center[j];
            }
        }
    }
    // 转换为RGB格式,记录日志,如果不需要日志,可以删除
    for(int i=0; i<54; i++)
    {
        // 默认白色
        rgb_color[i][0] = 200;
        rgb_color[i][1] = 200;
        rgb_color[i][2] = 200;
    }
    
    for(int j=0; j<5; j++)
    {
        uint8_t r,g,b;
        hsv2rgb(h_center[j], 1, 1, &r, &g, &b);
        printf("center=%.2f: ", h_center[j]);
        for(int i=0; i<group_count[j]; i++)
        {
            int rgb_color_index = group[j][i];
            rgb_color[rgb_color_index][0] = r;
            rgb_color[rgb_color_index][1] = g;
            rgb_color[rgb_color_index][2] = b;
            printf("%d ",group[j][i]);
        }
        printf("\n");
    }
    // 转换为字符串表示
    char cube_str[55];
    const char URFDLB[6] = {'U','R','F','D','L','B'};
    //   U
    // L F R B
    //   D
    for(int i=0; i<54; i++)
    {
        cube_str[i] = URFDLB[white_center_index / 9];
    }
    cube_str[54] = 0;
    int group_count_index = 0;
    int err = 0;
    for(int i=0; i<6; i++)
    {
        int center_index = 9 * i + 4;
        // 白色已经挑选好了,不需要再处理了
        if(center_index != white_center_index)
        {
            if(group_count[group_count_index] != 9)
            {
                printf("Error: Count of group %d is not 9.\n", group_count_index);
                err++;
            }
            int center_in_group = 0;
            for(int j=0; j<group_count[group_count_index]; j++)
            {
                int index = group[group_count_index][j];
                cube_str[index] =  URFDLB[i];
                if(index == center_index)
                {
                    center_in_group ++;
                }
            }
            if(center_in_group != 1)
            {
                printf("Error: Can not find center in group %d.\n", group_count_index);
                err++;
            }
            group_count_index++;
        }
    }
    printf("%s\n", cube_str);
    report_cube(fp, rgb_color, "聚类后的色块颜色:", cube_str);
    // 求解魔方
    if(err == 0)
    {
        int len = solve(cube_str, solution);
        if(len >= 0)
        {
            printf("Final solution is %s, length = %d\n", solution, len);
            fprintf(fp,"%s<br>\n", solution);
        }
        else
        {
            err++;
        }
    }  
    else
    {
        printf("Find error, skip solve(cube_str, solution)\n");
    }
    report_end(fp);
    return err;
}



void report_start(FILE *f)
{
    fprintf(f,
        "<!DOCTYPE html>\n"
        "<html><head><meta charset=\"UTF-8\"></head><body>\n"
        "<table><tr><td>\n"
        "<img src=\"out0.bmp\" height=300px><br>\n"
        "<img src=\"out1.bmp\" height=300px>\n"
        "</td><td>\n"
        );
}
void report_end(FILE *f)
{
    fprintf(f,
        "</td></tr></table></body></html>\n"
        );
}
void report_cube(FILE *f, uint8_t color[54][3], const char* text, const char* cube_str)
{
    const int map[9][12] = {
        {99,99,99, 0, 1, 2,99,99,99,99,99,99},
        {99,99,99, 3, 4, 5,99,99,99,99,99,99},
        {99,99,99, 6, 7, 8,99,99,99,99,99,99},
        {36,37,38,18,19,20, 9,10,11,45,46,47},
        {39,40,41,21,22,23,12,13,14,48,49,50},
        {42,43,44,24,25,26,15,16,17,51,52,53},
        {99,99,99,27,28,29,99,99,99,99,99,99},
        {99,99,99,30,31,32,99,99,99,99,99,99},
        {99,99,99,33,34,35,99,99,99,99,99,99},
    };
    fprintf(f,"%s<br>\n",text);
    fprintf(f,"<table>\n");
    for (int row = 0; row < 9; row++)
    {
        fprintf(f,"<tr>\n");
        for (int col = 0; col < 12; col++)
        {
            int r,g,b;
            char str[32];
            int index = map[row][col];
            if(index < 54)
            {
                r = color[index][0];
                g = color[index][1];
                b = color[index][2];
                if(cube_str)
                {
                    sprintf(str,"&nbsp;%c&nbsp;",cube_str[index]);
                }
                else
                {
                    sprintf(str,"%d",index);
                }
                
            }
            else
            {
                r = 255;
                g = 255;
                b = 255;
                sprintf(str," ");
            }
            fprintf(f,"<td style=\"background-color: rgb(%d, %d, %d);\">%s</td>\n",r,g,b,str);
        }
        fprintf(f,"</tr>\n");
    }

    fprintf(f,"</table>\n");
}
